#include "Punkt.h"
#include <math.h>

const long double PI = 3.141592653589793238462643;

Punkt::Punkt() {
    Kolor k(0, 0, 0);
    this->k = k;
    this->Obszar = 0;
//Etykieta = OB_niePrzydzielony;

}

/**
 * Oznaczenie zmiennych:
 *   r - odległość;
 *   teta - kąt w pionie; (zwiększa się jak się skaner cały podnosi)
 *   fi - kąt w poziomie; (to co dostaje z lasera);
 */
Punkt::Punkt(double xr, double yteta, double zfi, UkladWsp u) {

    Kolor k(0, 0, 0);
    this->k = k;
    this->Obszar = 0;
    //this->Etykieta = OB_niePrzydzielony;
    //this->Obszar = OB_niePrzydzielony;
    switch (u) {
        case XYZ:
            x = xr;
            y = yteta;
            z = zfi;

            break;
        case RTetaFi:
            r = xr;
            teta = yteta ; //pion
            fi = zfi + 6.5; //poziom
            //if(r > 7.0) setEtykieta(OB_zaDaleko);
// stare przeliczenia:
            x = r * cos(fi * PI / 180.0) * sin(teta * PI / 180.0);
            y = r * cos(fi * PI / 180.0) * cos(teta * PI / 180.0);
            z = r * sin(fi * PI / 180.0);
// z systemu geograficznego
//            x = r * cos(teta * PI / 180.0) * cos(fi * PI / 180.0 + PI/2);
//            y = r * cos(teta * PI / 180.0) * sin(fi * PI / 180.0 + PI/2);
//            z = r * sin(teta * PI / 180.0);
//zamiana od Jacka (orginalna).
//            x = r * sin(fi * M_PI / 180.0);
//            y = r * cos(teta * M_PI / 180.0) * cos(fi * M_PI / 180.0);
//            z = r * sin(teta * M_PI / 180.0) * cos(fi * M_PI / 180.0);
//zamiana od Jacka.
//            z = r * sin(fi * M_PI / 180.0);
//            y = r * cos(teta * M_PI / 180.0) * cos(fi * M_PI / 180.0);
//            x = r * sin(teta * M_PI / 180.0) * cos(fi * M_PI / 180.0);
            break;
    };

}

/**
 *
 *
 */
double Punkt::odleglosc(Punkt* pp) {
    return sqrt(
            (this->x - pp->x) * (this->x - pp->x)
            +(this->y - pp->y) * (this->y - pp->y)
            +(this->z - pp->z) * (this->z - pp->z)
            );
}

double Punkt::odleglosc(Punkt pp) {
    double r = sqrt(
            (this->x - pp.x) * (this->x - pp.x)
            +(this->y - pp.y) * (this->y - pp.y)
            +(this->z - pp.z) * (this->z - pp.z)
            );
    return r;
}
